% debug for no control 2r bots
clc;
clear;


%参数组初始化
initial_state = [pi/3 0 0; -2*pi/3 0 0];
is_g_enable = [1;1];
n_factor = 1;
tor_motor = jointMotor(1);

%这里使用的表达不同了，直接以水平为开始位置
pos_st = 0;
pos_ed = pi/2;
t_ed = 3;
simu_period = 1e-4;
simu_step = t_ed/simu_period;
sample_period = simu_period* 5;

a_dj = doubleJoint(initial_state,is_g_enable,n_factor);
t_acc = 1;
t_ed = 3;
pos_st = 0.1;
pos_ed = 0.15;
pos_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-pos_ed+pos_st);
a_value = fsolve(pos_final,0);
pos_s_traj_value = position_s_traj_info(a_value,t_acc,t_ed,simu_period,pos_st);


motor_res = zeros(2,length(1:simu_step),3);
state_info = zeros(2,length(1:simu_step),3);
cart_info = zeros(2,length(1:simu_step),2);

%% test-start

for i= 1: simu_step
    state_info(:,i,:) = a_dj.simu_loop([0;0],simu_period);
    jacobi_m = a_dj.get_jacobi(0);
    cart_info(:,i,:) = a_dj.get_cart_value(jacobi_m);
end


%%
anima =1;  % 0 for no animation ,just for debug
if(anima == 0)
    figure;
    plot(0:simu_period:(simu_step-1)*simu_period,cart_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
    hold on
    plot(0:simu_period:(simu_step-1)*simu_period,cart_info(2,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
    hold on
    legend('x','y');
    title("位移");
else
    n_points = simu_step;
    big_per = 10;
    figure;
    b_w  = 0.004;
    b_l  = 0.004;
    l_x = 0;
    l_y = 0;
    for n=1:big_per:n_points-(big_per-1)
        %绘图区
        l_x = a_dj.l*cos(state_info(1,n,1));
        l_y = a_dj.l*sin(state_info(1,n,1));
        plot([0 a_dj.l*cos(state_info(1,n,1));...
            l_x  l_x+a_dj.l*cos(state_info(1,n,1)+state_info(2,n,1))],...
            [0 a_dj.l*sin(state_info(1,n,1));...
            l_y l_y+a_dj.l*sin(state_info(2,n,1)+state_info(1,n,1))],'b','LineWidth',2); % Pendulum rod
        rectangle('Position',[-b_w/2,-b_l/2 ,b_w,b_l],'Curvature',0.1,'FaceColor',[1 0.1 0.1],'EdgeColor',[1 1 1]);
        rectangle('Position',[l_x-b_w/2,l_y-b_l/2 ,b_w,b_l],'Curvature',0.1,'FaceColor',[1 0.1 0.1],'EdgeColor',[1 1 1]);

        
        set(gca,'Ylim',[-0.2 0.2],'Xlim',[-0.1 2*a_dj.l]);
        drawnow;
    end
end
